#include <cstdlib>
#include <cmath>

#include <iostream>
#include <vector>

#include <boost/unordered_map.hpp>

#include "graph_data.h"
#include "spatial_index.h"
#include "graph_algo.h"
#include "graph_algo_adapters.h"

namespace
{
void vertex_to_cout(vertex_t const & v)
{
  std::cout << "V(ID" << v.id << " LAT" << v.data.lat << " LON" << v.data.lon
            << ")";
}
void edge_to_cout(edge_t const & e)
{
  std::cout << "E(S" << e.source << " D" << e.destination << " W" << e.weight
            << ")";
}

size_t count = 0;
struct out_iter
{
  out_iter operator ++()
  {
    return *this;
  }

  out_iter operator ++(int)
  {
    return *this;
  }

  out_iter& operator =(vertex_t const & v)
  {
    vertex_to_cout(v);
    ++count;
    std::cout << "->";
    return *this;
  }

  out_iter& operator *()
  {
    return *this;
  }
};


void print_vector(std::vector<vertex_t> const & vec)
{
  for (std::vector<vertex_t>::const_iterator it = vec.begin(); it != vec.end(); ++it)
  {
    std::cout << it->id << "<-";
  }
  
  std::cout << std::endl;
}

struct print_edge_f
{
  void operator () (edge_t const & e)
  {
    std::cout << e.source << " " << e.destination << " " << e.weight << std::endl;    
  }  
};

} // namespace

template<class DijkstraAdapter>
struct adapter_inserter_f
{
  adapter_inserter_f(graph_t const & graph, DijkstraAdapter & adapter, vertex_t source, bool forward)
    : graph_(graph), adapter_(adapter), source_(source), forward_(forward)
  {    
  }
  
  void operator () (edge_t edge)
  {
    vertex_t source = graph_.get_vertex_by_id(edge.source);
    vertex_t destination = graph_.get_vertex_by_id(edge.destination);
    
    std::pair<double, vertex_data_t> dist = get_dist(source_.data, source.data, destination.data);
    if (dist.first > 600.0)
      return;
    
    vertex_t v(INVALID_VERTEX_ID, dist.second);
    
    adapter_.decrease_key(v, dist.first, source_);
    if (!forward_)
      adapter_.decrease_key(source, dist.first + orthodromic_distance(v.data, source.data), v);
    else
      adapter_.decrease_key(destination, dist.first + orthodromic_distance(v.data, destination.data), v);        
  }
  
private:
  graph_t const & graph_;
  DijkstraAdapter & adapter_;
  vertex_t source_;    
  bool forward_;
};

int main()
{ 
  graph_t graph("/home/sa1nt/temp/graph/stpeter.graph");
  spatial_index_t index(graph, 100000, 100000);  
  
  std::pair<double, vertex_data_t> d = get_dist(vertex_data_t(10.0, 10.0), vertex_data_t(20.0, 20.0), vertex_data_t(5.0, 5.0));
  std::cout << d.first <<  " " << d.second.lon << " " << d.second.lat << std::endl;
  

  details::dijkstra_graph_t adapter(graph);
  details::dijkstra_graph_t adapter_2(graph);
  vertex_t source_vertex(INVALID_VERTEX_ID, geodata_t(30.2695, 59.8548));
  vertex_t destination_vertex(INVALID_VERTEX_ID, geodata_t(30.2882, 59.9362));
  adapter.decrease_key(source_vertex, 0.0, source_vertex);
  adapter_2.decrease_key(destination_vertex, 0.0, destination_vertex);
        
  index.for_all_nearest_edges(source_vertex.data, adapter_inserter_f<details::dijkstra_graph_t>(graph, adapter, source_vertex, true));  
  index.for_all_nearest_edges(destination_vertex.data, adapter_inserter_f<details::dijkstra_graph_t>(graph, adapter_2, destination_vertex, false));  

  std::cout << graph.get_vertex_by_id(123).data.lon << " " << graph.get_vertex_by_id(123).data.lat << std::endl;
  base_bidirectional_dijkstra(adapter, adapter_2, source_vertex, destination_vertex, out_iter());
}